home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / libs / sphigs / sph_dos.lha / dos / sphsrc / mat3vec.c < prev    next >
Encoding:
C/C++ Source or Header  |  1991-11-26  |  4.2 KB  |  149 lines
  1. /* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
  2.  
  3. /* --------------------------------------------------------------------------
  4.  * This file contains routines that operate on matrices and vectors, or
  5.  * vectors and vectors.
  6.  * -------------------------------------------------------------------------*/
  7.  
  8. #include "sphigslocal.h"
  9.  
  10. /* --------------------------  Static Routines    ---------------------------- */
  11.  
  12. /* -------------------------  Internal Routines  --------------------------- */
  13.  
  14. /* --------------------------  Public Routines    ---------------------------- */
  15.  
  16. /*
  17.  * Multiplies a vector by a matrix, setting the result vector.
  18.  * It assumes all homogeneous coordinates are 1.
  19.  * The two vectors involved may be the same.
  20.  */
  21.  
  22. void
  23. MAT3mult_vec(result_vec, vec, mat)
  24. MAT3vec      result_vec;
  25. register MAT3vec vec;
  26. register MAT3mat mat;
  27. {
  28.    MAT3vec        tempvec;
  29.    register double    *temp = tempvec;
  30.  
  31.    temp[0] =    vec[0] * mat[0][0] + vec[1] * mat[1][0] +
  32.         vec[2] * mat[2][0] +          mat[3][0];
  33.    temp[1] =    vec[0] * mat[0][1] + vec[1] * mat[1][1] +
  34.         vec[2] * mat[2][1] +          mat[3][1];
  35.    temp[2] =    vec[0] * mat[0][2] + vec[1] * mat[1][2] +
  36.         vec[2] * mat[2][2] +          mat[3][2];
  37.  
  38.    MAT3_COPY_VEC(result_vec, temp);
  39. }
  40.  
  41. /*
  42.  * Multiplies a vector of size 4 by a matrix, setting the result vector.
  43.  * The fourth element of the vector is the homogeneous coordinate, which
  44.  * may or may not be 1.  If the "normalize" parameter is TRUE, then the
  45.  * result vector will be normalized so that the homogeneous coordinate is 1.
  46.  * The two vectors involved may be the same.
  47.  * This returns zero if the vector was to be normalized, but couldn't be.
  48.  */
  49.  
  50. int
  51. MAT3mult_hvec(result_vec, vec, mat, normalize)
  52. MAT3hvec          result_vec;
  53. register MAT3hvec vec;
  54. register MAT3mat  mat;
  55. {
  56.    MAT3hvec             tempvec;
  57.    double        norm_fac;
  58.    register double    *temp = tempvec;
  59.    register int     ret = TRUE;
  60.  
  61.    temp[0] =    vec[0] * mat[0][0] + vec[1] * mat[1][0] +
  62.         vec[2] * mat[2][0] + vec[3] * mat[3][0];
  63.    temp[1] =    vec[0] * mat[0][1] + vec[1] * mat[1][1] +
  64.         vec[2] * mat[2][1] + vec[3] * mat[3][1];
  65.    temp[2] =    vec[0] * mat[0][2] + vec[1] * mat[1][2] +
  66.         vec[2] * mat[2][2] + vec[3] * mat[3][2];
  67.    temp[3] =    vec[0] * mat[0][3] + vec[1] * mat[1][3] +
  68.         vec[2] * mat[2][3] + vec[3] * mat[3][3];
  69.  
  70.    /* Normalize if asked for, possible, and necessary */
  71.    if (normalize) {
  72.       if (MAT3_IS_ZERO(temp[3])) {
  73. #ifndef THINK_C
  74.      fprintf (stderr,
  75.           "Can't normalize vector: homogeneous coordinate is 0");
  76. #endif
  77.      ret = FALSE;
  78.       }
  79.       else {
  80.      norm_fac = 1.0 / temp[3];
  81.      MAT3_SCALE_VEC(result_vec, temp, norm_fac);
  82.      result_vec[3] = 1.0;
  83.       }
  84.    }
  85.    else MAT3_COPY_HVEC(result_vec, temp);
  86.  
  87.    return(ret);
  88. }
  89.  
  90. /*
  91.  * Sets the first vector to be the cross-product of the last two vectors.
  92.  */
  93.  
  94. void
  95. MAT3cross_product(result_vec, vec1, vec2)
  96. MAT3vec      result_vec;
  97. register MAT3vec vec1, vec2;
  98. {
  99.    MAT3vec        tempvec;
  100.    register double    *temp = tempvec;
  101.  
  102.    temp[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
  103.    temp[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
  104.    temp[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
  105.  
  106.    MAT3_COPY_VEC(result_vec, temp);
  107. }
  108.  
  109. /*
  110.  * Finds a vector perpendicular to vec and stores it in result_vec.
  111.  * Method:  take any vector (we use <0,1,0>) and subtract the
  112.  * portion of it pointing in the vec direction.  This doesn't
  113.  * work if vec IS <0,1,0> or is very near it.  So if this is
  114.  * the case, use <0,0,1> instead.
  115.  * If "is_unit" is TRUE, the given vector is assumed to be unit length.
  116.  */
  117.  
  118. #define SELECT    .7071    /* selection constant (roughly .5*sqrt(2) */
  119.  
  120. void
  121. MAT3perp_vec(result_vec, vec, is_unit)
  122. MAT3vec result_vec, vec;
  123. int    is_unit;
  124. {
  125.    MAT3vec    norm;
  126.    double    dot;
  127.  
  128.    MAT3_SET_VEC(result_vec, 0.0, 1.0, 0.0);
  129.  
  130.    MAT3_COPY_VEC(norm, vec);
  131.  
  132.    if (! is_unit) MAT3_NORMALIZE_VEC(norm, dot);
  133.  
  134.    /* See if vector is too close to <0,1,0>.  If so, use <0,0,1> */
  135.    if ((dot = MAT3_DOT_PRODUCT(norm, result_vec)) > SELECT || dot < -SELECT) {
  136.       result_vec[1] = 0.0;
  137.       result_vec[2] = 1.0;
  138.       dot = MAT3_DOT_PRODUCT(norm, result_vec);
  139.    }
  140.  
  141.    /* Subtract off non-perpendicular part */
  142.    result_vec[0] -= dot * norm[0];
  143.    result_vec[1] -= dot * norm[1];
  144.    result_vec[2] -= dot * norm[2];
  145.  
  146.    /* Make result unit length */
  147.    MAT3_NORMALIZE_VEC(result_vec, dot);
  148. }
  149.